#include "bsp_mecanum.h"
#include "stdlib.h"

// 限制数值范围
int16_t constrain_value(int16_t value, int16_t min_val, int16_t max_val) {
    if (value > max_val)
        return max_val;
    if (value < min_val)
        return min_val;
    return value;
}

// 麦克纳姆轮运动控制,通过控制四轮速度,美国手，返回麦克纳姆轮的四轮速度-100~100
void mecanum_caculus(Mecanum_Speed* mecanum_speed, UniRemote_HandleTypeDef* hur) {
    // 死区处理
    uint8_t min_value = 10;  // 10%
    // 计算各方向分量（摇杆范围：-100 ~ +100）

    int16_t vx = abs(hur->left_x) < min_value ? 0 : hur->left_x;    // 左手左右横向平移
    int16_t vy = abs(hur->left_y) < min_value ? 0 : hur->left_y;    // 左手上下前后移动
    int16_t vr = abs(hur->right_x) < min_value ? 0 : hur->right_x;  // 右手左右定义旋转

    // 麦克纳姆轮运动学公式
    mecanum_speed->lf_speed = vy + vx + vr;  // 左前轮: 前+右+顺时
    mecanum_speed->rf_speed = vy - vx - vr;  // 右前轮: 前-右-顺时
    mecanum_speed->lb_speed = vy - vx + vr;  // 左后轮: 前-右+顺时
    mecanum_speed->rb_speed = vy + vx - vr;  // 右后轮: 前+右-顺时

    // 限制速度在 -100 ~ +100 范围内
    mecanum_speed->lf_speed = constrain_value(mecanum_speed->lf_speed, -100, 100);
    mecanum_speed->rf_speed = constrain_value(mecanum_speed->rf_speed, -100, 100);
    mecanum_speed->lb_speed = constrain_value(mecanum_speed->lb_speed, -100, 100);
    mecanum_speed->rb_speed = constrain_value(mecanum_speed->rb_speed, -100, 100);
}
